from __future__ import division
import math

STRAIGHTS = (0, 1, 2, 3, 0)
DIRECTIONS = (0, 1, 2, 3, 4, 5, 6, 7)
INTERS = ((0, 1), (1, 0), (0, -1), (-1, 0), (0, 1))
MOVES = [(0, 1), (1, 0), (0, -1), (-1, 0), (1, 1), (1, -1), (-1, -1), (-1, 1)]
REV = (2, 3, 0, 1)
COSTS = (1, 1.4142135623730951)

def add(t1, t2):
	return t1[0] + t2[0], t1[1] + t2[1]

def subtract(t1, t2):
	return t1[0] - t2[0], t1[1] - t2[1]

def slide(container, width):
	for i in xrange(len(container)-1):
		yield container[i:i+width]

def motion_time(s, a, u=0, max_v=None):
	if a > 0 and max_v is not None:
		v = motion_speed(s, a, u)
		if v > max_v:
			m = (max_v**2 - u**2) / (a*2)
			time = motion_time(m, a, u, max_v) + motion_time(s-m, 0, max_v, max_v)
		else:
			v = motion_speed(s, a, u, max_v)
			time = (v - u) / a
	elif a != 0:
		v = motion_speed(s, a, u, max_v)
		time = (v - u) / a
	else:
		time = s / u
	
	return time

def motion_speed(s, a, u=0, max_v=None):
	if a > 0 and max_v is not None:
		speed = min(max_v, math.sqrt(u**2 + 2*a*s))
	else:
		speed = math.sqrt(u**2 + 2*a*s)
	
	return speed

def mass_acceleration(mass, drive, gravity, incline, friction, downhill):
	down = mass * gravity
	down *= math.sin(incline)
	
	normal = mass * gravity * math.cos(incline)
	cross = 9.827
	
	frictional = 0.787 * normal
	
	if not downhill:
		uphill = drive
		force = uphill - down
		force -= frictional
	else:
		uphill = 0
		down += drive
		force = down - uphill
		force -= frictional
	
	acceleration = force / mass
	
	return acceleration

class PathingMap(object):
	def reachable(self, unit, turntime):
		times = {unit.position: 0}
		starts = {unit.position: unit.position}
		changed = set((unit.position,))
		
		while changed:
			for position in changed.copy():
				for direction in DIRECTIONS:
					if self.walkable(position, direction, unit):
						dest = add(position, MOVES[direction])
						dx, dy = dest
						
						if not (0 <= dx < self.width and 0 <= dy < self.height):
							continue
						
						time = self.walk_time(position, direction, unit)
						
						if times[position] + time > turntime:
							continue
						
						if dest not in times or times[dest] > times[position] + time:
							times[dest] = times[position] + time
							starts[dest] = position
							changed.add(dest)
				changed.remove(position)
		
		return times, starts

class MechanicsPathingMap(PathingMap):
	"""A not so good pathing map that attempts to use mechanics to calculate
	pathing costs."""
	
	def __init__(self, heights, inclines):
		self.width, self.height = len(inclines[0]), len(inclines)
		self.cell_heights = heights
		self.edge_inclines = inclines
	
	def standable(self, position, unit):
		x, y = position
		incline = min(self.edge_inclines[y][x])
		
		return (mass_acceleration(unit.mass, unit.engine, 9.8, incline, 0, True) > 0 
			and mass_acceleration(unit.mass, unit.engine, 9.8, incline, 0, False) > 0)
	
	def walkable(self, position, direction, unit):
		# 704 701
		# 3 1 6 2
		# 625 543
		#threshold=radians(35)
		
		ox, oy = position
		dx, dy = add(position, MOVES[direction])
		
		if not (0 <= dx < self.width and 0 <= dy < self.height):
			return False
		
		if direction <= 3:
			origin, dest = self.edge_inclines[oy][ox], self.edge_inclines[dy][dx]
			incline = max(origin[direction], dest[REV[direction]])
			
			downhill = self.cell_heights[oy][ox] > self.cell_heights[dy][dx]
			return (mass_acceleration(unit.mass, unit.engine, 9.8, incline, 0, downhill) > 0 
				and mass_acceleration(unit.mass, unit.engine, 9.8, incline, 0, not downhill) > 0)
		else:
			#return False
			dir1 = STRAIGHTS[direction-4]
			dir2 = STRAIGHTS[direction-3]
			
			inter1 = add(position, INTERS[direction-4])
			inter2 = add(position, INTERS[direction-3])
			
			return ((self.walkable(position, dir2, unit) and self.walkable(inter2, dir1, unit))
				or (self.walkable(position, dir1, unit) and self.walkable(inter1, dir2, unit)))
	
	def walk_time(self, position, direction, unit, u=0):
		ox, oy = position
		dx, dy = add(position, MOVES[direction])
		
		time = 0
		
		if direction <= 3:
			incline1 = self.edge_inclines[oy][ox][direction]
			incline2 = self.edge_inclines[dy][dx][REV[direction]]
			
			downhill = self.cell_heights[oy][ox] > self.cell_heights[dy][dx]
			# Leaving current incline:
			
			a = mass_acceleration(unit.mass, unit.engine, 9.8, incline1, 0, downhill)
			u = 0
			s = 16/2 * 32
			
			v = motion_speed(s, a, u)
			time += motion_time(s, a, u)
			
			# Entering next incline:
			a = mass_acceleration(unit.mass, unit.engine, 9.8, incline2, 0, downhill)
			u = v
			s = 16/2 * 32
			
			#print motion_time(s, a, u)
			#time += motion_time(s, a, u)
			
			return time
		else:
			dir1 = STRAIGHTS[direction-4]
			dir2 = STRAIGHTS[direction-3]
			
			inter1 = add(position, INTERS[direction-4])
			inter2 = add(position, INTERS[direction-3])
			
			if self.walkable(position, dir2, unit) and self.walkable(inter2, dir1, unit):
				return COSTS[1] * (self.walk_time(position, dir2, unit) + self.walk_time(inter2, dir1, unit)) / 2
			else:
				return COSTS[1] * (self.walk_time(position, dir1, unit) + self.walk_time(inter1, dir2, unit)) / 2
		
		return COSTS[direction // 4]
	
	def path_time(self, cells, unit, conserve_speed=False):
		time = 0
		u = 0
		for start, dest in slide(cells, 2):
			direction = MOVES.index(subtract(start, dest))
			time += walk_time(start, direction, unit)
		return time

class SimplePathingMap(PathingMap):
	def __init__(self, heights, inclines):
		self.width, self.height = len(inclines[0]), len(inclines)
		self.cell_heights = heights
		self.edge_inclines = inclines
	
	def standable(self, position, unit):
		x, y = position
		incline = min(self.edge_inclines[y][x])
		
		return incline < math.radians(35)
	
	def walkable(self, position, direction, unit):
		# 704 701
		# 3 1 6 2
		# 625 543
		#threshold=radians(35)
		
		ox, oy = position
		dx, dy = add(position, MOVES[direction])
		
		if not (0 <= dx < self.width and 0 <= dy < self.height):
			return False
		
		if direction <= 3:
			origin, dest = self.edge_inclines[oy][ox], self.edge_inclines[dy][dx]
			incline = max(origin[direction], dest[REV[direction]])
			
			return incline < math.radians(35)
		else:
			#return False
			dir1 = STRAIGHTS[direction-4]
			dir2 = STRAIGHTS[direction-3]
			
			inter1 = add(position, INTERS[direction-4])
			inter2 = add(position, INTERS[direction-3])
			
			return ((self.walkable(position, dir2, unit) and self.walkable(inter2, dir1, unit))
				or (self.walkable(position, dir1, unit) and self.walkable(inter1, dir2, unit)))
	
	def walk_time(self, position, direction, unit, u=0):
		ox, oy = position
		dx, dy = add(position, MOVES[direction])
		
		time = 0
		
		if direction <= 3:
			incline1 = self.edge_inclines[oy][ox][direction]
			incline2 = self.edge_inclines[dy][dx][REV[direction]]
			
			downhill = self.cell_heights[oy][ox] > self.cell_heights[dy][dx]
			# Leaving current incline:
			
			distance = 32
			
			time = COSTS[0] * distance
			
			return time
		else:
			dir1 = STRAIGHTS[direction-4]
			dir2 = STRAIGHTS[direction-3]
			
			inter1 = add(position, INTERS[direction-4])
			inter2 = add(position, INTERS[direction-3])
			
			distance = 32
			time = COSTS[1] * distance
			
			return time
		
		return COSTS[direction // 4]
	
	def path_time(self, cells, unit, conserve_speed=False):
		time = 0
		u = 0
		for start, dest in slide(cells, 2):
			direction = MOVES.index(subtract(start, dest))
			time += self.walk_time(start, direction, unit)
		return time

PathingMap = SimplePathingMap
